/*!  \file  Matrix.cpp
 *	  \brief  
 * 
 *   \details   details
 *   \author    Yordan Madzhunkov German Research School for Simulation Sciences GmbH
 *   \version   1.0
 *   \date      04/26/2013 05:57:01 PM
 *   \copyright GNU Public License.
 */

#include <iostream>
#include <assert.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>
#include "Matrix.h"

#include "blas.h"

#include "macros.h"
#include <iomanip>      // std::setw
extern "C" {
void dormbr_	(	
const char 		* 	vect,
const char 		* 	side,
const char 		* 	trans,
const int 		* 	m,
const int 		* 	n,
const int 		* 	k,
const double  	* 	a,
const int 		* 	lda,
const double  	* 	tau,
		double  	* 	c,
const int 		* 	ldc,
		double  	* 	work,
const int 		* 	lwork,
		int 		* 	info 
);
void dorgbr_ 	(	
const char		* 	vect,
const int 		* 	m,
const int 		* 	n,
const int 		* 	k,
const double  	* 	a,
const int 		* 	lda,
const double  	* 	tau,
		double  	* 	work,
const int 		* 	lwork,
		int 		* 	info 
);

}
using namespace std;

Matrix::Matrix(const int m_, const int n_, const Type type_, const double * const data_){
	data = 0; type = type_; m = m_; n = n_;  
	M = m; N = n; shift = 0;
	order = RowMajor;
	setLD();
	relflectorStorage = NULL;
	rotationIndex     = NULL;
	Q						= true;
	if (allocateStorage() ) 
		if (data_) 
			initialize(data_);
	assert(isFullMatrix());
}
void Matrix::transpose  () {
	if (isFullMatrix()) {
		int k = n; n = m; m = k;
		k = N; N = M; M = k;
		order = isRowMajor() ? ColMajor : RowMajor; 
		setLD();
	} else {
		int i = shift; int j = shift;
		if (isDense()) getRowColFromOffset(shift, i, j);
		int n_ = n, m_ = m;
		resetView();
		assert(isFullMatrix());
		transpose();
		setView(j, i, n_, m_);
		ld = lda();
	}
}
void Matrix::copy   (const Matrix & mat) {
	type    			= mat.type; 
	m       			= mat.m;
	n       			= mat.n;
	M       			= mat.M;
	N       			= mat.N;
	shift   			= mat.shift;
	ld		  			= mat.ld;
	delocateStorage();
	data    			= mat.data;
	counter 			= mat.counter; *counter += 1;
	order   			= mat.order;
	relflectorStorage = mat.relflectorStorage;
	Q						= mat.Q;
}
void Matrix::copyOnlyData (const Matrix & mat){ // keep the current storage
	if (!sameDim(mat)) return;
	if (isDense() && mat.isDense() ) {
		if (sameOrder(mat)){ // the faster option {
			const int limit1 = isRowMajor() ? rows() : cols();
			const int limit2 = isRowMajor() ? cols() : rows();
			const int gap1   = gap()     + limit2;
			const int gap2   = mat.gap() + limit2;
			double       * RS tmp  = data     ;
			const double * RS tmp2 = mat.getData();
			for (int i = 0; i < limit1; i++) {
				memcpy(tmp, tmp2, limit2 * sizeof(*tmp));
				tmp  += gap1;	tmp2 += gap2;
			}
		} else {
			const int limit1 = isRowMajor() ? rows() : cols();
			const int limit2 = isRowMajor() ? cols() : rows();
			const int gap1   = gap()     ;
			const int inc2   = mat.lda() ;
			double       * RS tmp  = data     ;
			const double * RS tmp2 = mat.getData();
			const double * RS tmp3 = 0;
			for (int i = 0; i < limit1; i++) {
				tmp3 = tmp2 + i;
				for (int j = 0; j < limit2; j++) {
					*tmp = *tmp3; tmp++; tmp3 += inc2;
				}
				tmp  += gap1;
			}
		}
	} else {
		if (isSparse() && sameType(mat)) {
			// copy the diagonal
			double       * RS tmp  = data     ;
			const double * RS tmp2 = mat.getData();
			int l = minDim();
			memcpy(tmp, tmp2, l * sizeof(*tmp));
			if (!isDiagonal()) {
				tmp  = getModOffDiagonal();
				tmp2 = mat.getOffDiagonal();
				memcpy(tmp, tmp2, (l-1) * sizeof(*tmp));
			}
		} else {
			MISSING
		}
	}

}
void Matrix::copyWithData (const Matrix & mat) {
	copy(mat);
	if (data) {
		delocateStorage();
		M = m = mat.rows();
		N = n = mat.cols();
		setLD();
		allocateStorage();
		if (mat.isFullMatrix()) {
			if (data) initialize(mat.data);
		}	else {
			if (data) copyOnlyData(mat);
		}
	}
}
void Matrix::makeRowMajor() { 
	if (isColMajor()) {
		assert(isFullMatrix()); 
		order = RowMajor; 
		setLD(); 
	};
};
void Matrix::makeColMajor() { 
	if (isRowMajor()) {
		assert(isFullMatrix()); 
		order = ColMajor; 
		setLD();
	} 
};
Matrix::Matrix(const Matrix & mat, const Operation op) {
	data = 0; counter = 0;
	switch (op) {
		case Transpose    : { copy(mat);	transpose(); break;	}
		case Copy 		   : { copy(mat);	break;}
		case CopyWithData : { copyWithData(mat); break;}
		case TransposeData: { copy(mat); changeOrder(); break; } 
		default: {	assert(false);	}
	}
}
Matrix & Matrix::operator= (const Matrix & mat){
	if ( &mat == this) return *this;
	copy(mat);
	return *this;	
}
Matrix::Matrix(const Matrix & mat) {data = 0; counter = 0;	copy(mat);}

unsigned int Matrix::getStorageSize() const {
	assert(isFullMatrix()); 
	switch (type) {
		case Orthogonal  : {}
		case General     : { return rows() * cols();  };
		case Diagonal    : { return minDim();         };
		case BiDiagonal  : { return minDim() * 2 - 1; };
		case TriDiagonal : { return minDim() * 2 - 1; };
		case ElementaryReflectors : { return minDim();};
		default          : { return 0; };
	}
}
void Matrix::setView(const int i_, const int j_, const int m_, const int n_) {
	if (isSubMatrix()) resetView();
	assert(shift == 0);
	assert(i_ + m_ <= M); 
	assert(j_ + n_ <= N); 
	if (isDense()) {
		shift = offset(i_,j_);
#ifndef NDEBUG
		int i1, i2;
		getRowColFromOffset(shift, i1, i2);
		assert(i1 == i_ && i2 == j_);	
#endif
		data    += shift; 
		n = n_; m = m_;
	} else {
		if ( i_ == j_ ) {
			shift = i_;
			data += shift;
			n = n_; m = m_;
		} else {
			cerr << endl;
			cerr <<  __PRETTY_FUNCTION__ ; 
			cerr << " setView only allowed on diagonal" << endl;
		}

	}
	// ld stays the same
}
void Matrix::resetView() {
	data -= shift; shift = 0;
	m = M;
	n = N;
	assert(isFullMatrix());
}
bool Matrix::allocateStorage(){
	assert(isFullMatrix());
	if (data) delete [] data;
	counter = new int; 
	if (counter) *counter = 1;
	try  {
		data = new double[getStorageSize()];
  	} catch (std::bad_alloc& ba) {
    	cerr << __PRETTY_FUNCTION__  << "bad_alloc: " << ba.what() << endl;
   }
	return data != NULL && counter != NULL;
}
void Matrix::delocateStorage() {
	if ( data ) {
		if (!isFullMatrix())
			resetView(); 
//			cout << *this << endl;
		assert(isFullMatrix());
		if (counter) { 
			*counter = *counter - 1;
			if (data && *counter == 0) { 
				delete [] data; 
				delete counter; 
				counter = 0;
			}
		}
		data = 0;
	} else {
		assert(counter == 0);
	}
};

void Matrix::fill(const double val) {
	if (isGeneral() || isOrthogonal() ) {
		const int limit1 = isRowMajor() ? rows() : cols();
		const int limit2 = isRowMajor() ? cols() : rows();
		const int hop    = gap();
		double * RS tmp  = isFullMatrix() ? getModData() : data;
		for (int i = 0; i < limit1; i++) {
			for (int j = 0; j < limit2; j++) {
				*tmp = val; tmp++;
			}
			tmp += hop;
		}
	} else {
		double * RS tmp = getModData();
		if (isDiagonal() || isReflector()) {
			for (int i = 0; i < minDim(); i++) 
				tmp[i] = val;
		} else if (isBiDiagonal() || isTriDiagonal()) {
			double * RS tmp2  = getModOffDiagonal();
			for (int i = 0; i < minDim() - 1; i++) {
				tmp[i] = val; tmp2[i] = val;
			};
			tmp[minDim() - 1] = val;
		} else {
			MISSING
		}
	}
}
void Matrix::initialize(const double * const source) {
	assert(isFullMatrix());
	if (data != 0 && source != 0) 
		memcpy(getModData(), source, getStorageSize() * sizeof(double)); 
}
void Matrix::add(const Matrix & mat, const double alpha) {
	if (sameDim(mat)) { // the mathematical definition requires this :) 
		if (isDense() && mat.isDense()) { // both are dense
			if (sameOrder(mat)){ // the faster option 
				const int limit1 = isRowMajor() ? rows() : cols();
				const int limit2 = isRowMajor() ? cols() : rows();
				const int gap1   = gap()     ;
				const int gap2   = mat.gap() ;
				double       * RS tmp  = data     ;
				const double * RS tmp2 = mat.getData();
				for (int i = 0; i < limit1; i++) {
					for (int j = 0; j < limit2; j++) {
						*tmp += alpha * *tmp2; tmp++; tmp2++;
					}
					tmp  += gap1;	tmp2 += gap2;
				}
			} else {
				const int limit1 = isRowMajor() ? rows() : cols();
				const int limit2 = isRowMajor() ? cols() : rows();
				const int gap1   = gap()     ;
				const int inc2   = mat.lda() ;
				double       * RS tmp  = data     ;
				const double * RS tmp2 = mat.getData();
				const double * RS tmp3 = 0;
				for (int i = 0; i < limit1; i++) {
					tmp3 = tmp2 + i;
					for (int j = 0; j < limit2; j++) {
						*tmp += alpha * *tmp3; 
						tmp++; tmp3 += inc2;
					}
					tmp  += gap1;
				}
			}
			makeGeneral();
		} else {
			if (isDense() && mat.isSparse()) {
				for (int i=0; i < minDim();i++)
					data[offset(i,i)] += alpha * mat.getData()[i];
				bool addUp   = mat.isBiDiagonal() &&   mat.isUpper();
				bool addDown = mat.isBiDiagonal() && (!mat.isUpper());
				addUp   |= mat.isTriDiagonal();
				addDown |= mat.isTriDiagonal();
				const double * b = mat.getOffDiagonal();
				const int l = mat.minDim() - 1;
				if ( addUp   ) for (int i=0; i < l;i++)
						data[offset(i,i+1)] += alpha * b[i];
				if ( addDown ) for (int i=0; i < l;i++)
						data[offset(i+1,i)] += alpha * b[i];
			} else if (isSparse() && mat.isSparse()) {
				if (sameType(mat)) {
					const unsigned int l = mat.minDim() - 1;
					if (isDiagonal()) {
						for (unsigned int i=0; i <= l;i++)
							data[i] += alpha * mat.getData()[i];
					} else {
						const double * RS b2 = mat.getOffDiagonal();
						double       * RS b  =  getModOffDiagonal();
						for (unsigned int i=0; i < l;i++) {
							data[i] += alpha * mat.getData()[i];
							b   [i] += alpha * b2[i];
						}
						data[l] += alpha * mat.getData()[l];
					}
				} else {
					MISSING
				}
			} else { assert(isSparse() && mat.isDense());
				Matrix C(rows(), cols(), General);
				C.fill(0);
				C.add(*this, 1.0);
				C.add(mat, alpha);
				*this = C;
			}
		}
	} else {
		cerr << "trying to add matrices with different dimentions" << endl;
		cout << *this << mat << endl;
	}
}
bool Matrix::changeOrder(){ // changes the actual memory layout
	if (isFullMatrix()) {
		if (data && isDense()) { // if there is no data we can just flip it
			Matrix tmp = *this;
			delocateStorage(); allocateStorage();
			order = order == RowMajor ? ColMajor : RowMajor;
			setLD();
			copyOnlyData(tmp);
			return true;
		}
	} else {
		if (data && isDense()) {
			if ( rows() == cols() ) {
				int n = rows();
				int ld2 = isRowMajor() ? M : N;
				for (int i = 0; i < n; i++) {
					double * RS tmp  = data + i * ld;
					double * RS tmp2 = data + i + (i+1) * ld2;
					for (int j = i+1; j < n; j++) {
						swap(*tmp, *tmp2);
						tmp++; tmp2 += ld2;
					}
				}
				return true;
			} else {
				MISSING;
			}
		}
		return false;
	}
	return false;
}
double Matrix::nrm2() const {
	if( isReflector() ) return 0;
	if (isGeneral() || isOrthogonal()) {
		int limit1 = isRowMajor() ? rows() : cols();
		int limit2 = isRowMajor() ? cols() : rows();
		double norm = 0;
		for (int i = 0; i < limit1; i++) {
			const double * tmp = getData() + i * ld;
			double norm1 = 0;
			for (int j = 0; j < limit2; j++) {
				norm1 += (*tmp) * (*tmp);
				tmp++;
			}
			norm += norm1;
		}
		return sqrt(norm);
	}
	int size = minDim();		int one = 1;	
	double norm1 =  dnrm2_(&size, data, &one);
	if (isDiagonal()) return norm1;
	int n1 = size - 1;
	double norm2 = dnrm2_(&n1, getOffDiagonal(), &one); 
	double fact = isBiDiagonal() ? 1 : 2;
	return sqrt(norm1 * norm1 + fact * norm2 * norm2);
}
Matrix & Matrix::operator *= (const Matrix & m) {
	// note that this implementation should not be the most efficient one
	assert(cols() == m.rows());
	// check if data pointer is shared with other matrices
	double * d = getModData();
	assert(count() == 1);
	switch (type) {
		case Orthogonal : {		}
		case General : {
			switch (m.type) {
				case Orthogonal : {}
				case General : {	Matrix C = *this * m; *this = C;	break; }
				case Diagonal : {	
					if ( m.cols() == cols() ) { // we don't need to change storage
						DiagMultCols(m.data);
					} else {	MISSING }
					return *this;
				}
				case BiDiagonal  : { MISSING; break; }
				case TriDiagonal : { MISSING; break; }
				case GivensRotations : {MISSING; break;};
				case ElementaryReflectors : {MISSING; break; };
				default          : { MISSING; break; }
			}
			break;
		}
		case Diagonal : {
			switch (m.type) {
				case Orthogonal : {}
				case General : { return *this = (m.T() * this->T()).T(); }
				case Diagonal : {
					if (isFullMatrix() ) {
						int limit = min(minDim(), m.minDim());
						for (int i = 0; i < limit; i++) 
							d[i] *= m.data[i];
						if (limit < minDim() ) {
							double * newData = new double[limit];
							if ( newData) {
								memcpy(newData, data, sizeof(newData[0]) * limit);
								if (data) delete [] data; data = 0;
								data = newData; newData = 0;
							}	
						}
						n = m.cols();
						N = n; M = this->m;
						setLD();
						assert(isFullMatrix());
					} else {
						MISSING
					}
					break;
				}
				case BiDiagonal  : { MISSING; break; }
				case TriDiagonal : { MISSING; break; }
				case GivensRotations : {MISSING; break;};
				case ElementaryReflectors : {MISSING; break; };
			}
			break;
		}
		case BiDiagonal  : { MISSING; break; }
		case TriDiagonal : { MISSING; break; }
		case GivensRotations : {MISSING; break;};
		case ElementaryReflectors : {MISSING; break; };
		default : {	
			cerr << "[Matrix::operator*=] ERROR unkown type	" << endl;	
		}
	}
	return *this;
}
void Matrix::generateOrthogonal() {
	if (!isReflector()) return;
	const Matrix * refl = getRefl();
	char vect = isQ() ? 'Q' : 'P';
	int M = rows();
	int N = cols();
	int K = isQ() ? refl->cols() : refl->rows(); 

	Matrix A2( *refl, CopyWithData);

	const double *	  a   = A2.getData();
	int 			   lda   = A2.lda();
	const double *	tau   = getModData();
	double * work = NULL; double wkopt = 0;
	int lwork = -1, info = 0;
	dorgbr_( &vect, &M, &N, &K, a, &lda, tau, &wkopt, &lwork, &info);
	if (info != 0) return;
	lwork = (int) wkopt;
	work = new double[lwork];
	if ( work ) {
		dorgbr_( &vect, &M, &N, &K, a, &lda, tau, work, &lwork, &info);
		delete [] work; work = 0;
		if (info == 0) { // result is stored in A2
			if (sameDim(A2)) { // easy one
				*this = A2;
				makeOrthogonal();
			} else {
				MISSING;
			}	
		}
		assert(info == 0);
	}
}; // reflector to orthogonal
void Matrix::applyReflector(const Matrix & Refl, const bool fromLeft) {
	if (!Refl.isReflector()) return;
	if (isColMajor()) {
		const Matrix * refl = Refl.getRefl();
		char vect  = Refl.isQ()         ? 'Q' : 'P';
		char side  = fromLeft           ? 'L' : 'R';
		char trans = Refl.isRowMajor()  ? 'N' : 'T';
		assert(refl->isColMajor()); 
		assert(Refl.cols() == Refl.rows()); 
		assert( ( fromLeft  && Refl.cols() == rows()) ||
				  ( !fromLeft && Refl.rows() == cols()) );
				
		if (refl) {
			int M = rows();
			int N = cols();
			int K = Refl.isQ() ? refl->cols() : refl->rows(); 
			const double *	  a   = refl->getData();
			int 			   lda   = refl->lda();
			const double *	tau   = Refl.getData();
			double       *   c   = getModData();
			int 		      ldc   = this->lda();
			double  	    *	work  = NULL;
			double 			wkopt = 0;
			int 		     lwork  = -1;
			int 		 	   info  = 0;
			//query work space
			dormbr_	( &vect, &side, &trans, &M, &N, &K, a, &lda,
							tau, c, &ldc, &wkopt, &lwork, &info );
			assert(info == 0);
			lwork = (int) wkopt;
			work = new double[lwork];
			if ( work ) {
				dormbr_	( &vect, &side, &trans, &M, &N, &K, a, &lda,
								tau, c, &ldc, work, &lwork, &info );
				assert(info == 0);
				delete [] work; work = 0;
			}
		}
	} else {
		changeOrder();
		applyReflector(Refl, fromLeft);
	}
}
const Matrix Matrix::operator*(const Matrix & mat) const {
	const Matrix & A = *this;
	const Matrix & B = mat;
	if (isDense()) {
		if (mat.isDense()) {
			assert(A.cols() == B.rows());
			Type newType = A.isOrthogonal() && B.isOrthogonal() ?
								Orthogonal : General;

			if ((A.isReflector() && B.isOrthogonal()) || 
				 (B.isReflector() && A.isOrthogonal()) )
					newType = Orthogonal;

			Matrix C(B.cols(), A.rows(), newType); // allocate mem for the Cult
			C.transpose(); // col major
			assert(A.rows() == C.rows() && B.cols() == C.cols());
			assert(C.order == ColMajor);

			if ( !isReflector() && !mat.isReflector()) {
				char transA = A.isColMajor() ? 'N' : 'T';
				char transB = B.isColMajor() ? 'N' : 'T';
				double alpha = 1.0; double beta = 0.0;
				int lda = A.lda(), ldb = B.lda(), ldc = C.lda();
				int M = C.rows(), K = A.cols(), N = C.cols();
				if ( transA == 'N' ) {	assert(lda >= max(1,M));
				} else {			assert(lda >= max(1,K));	}
         
				dgemm_(&transA, &transB, &M, &N, &K, &alpha, A.data, &lda,
						B.data, &ldb, &beta, C.data, &ldc);
				assert(C.order == ColMajor); // blas stores in ColMajor format
 	 	 		return C;	
			} else if ( isReflector() || mat.isReflector()) {
					assert(newType == Orthogonal || newType == General);
					if (isReflector() && !B.isReflector()) {
						C.copyOnlyData(B);
						C.applyReflectorFromLeft(A);
					} else if (!isReflector() && B.isReflector()) {
						C.copyOnlyData(A);
						C.applyReflectorFromRight(B);
					} else { // both of them are :) 
						C.makeIdentity();
						C.applyReflectorFromLeft(A);
						C.applyReflectorFromRight(B);
					}
					return C;
			} else {
					MISSING;
			}
		} else {	assert(mat.isSparse()); // we know it's sparse
			if (B.isDiagonal()) {
				Matrix C(A.rows(), B.cols(), General);
				assert(C.isRowMajor() && A.cols() == B.rows());
				double * cD = C.getModData();
				for (int r = 0; r < C.rows(); r++)
					for (int c = 0; c < C.cols(); c++) {
						if( r < A.rows() && c < A.cols() &&
							 c < B.cols() && c < B.rows() )
						cD[C.offsetRowMajor(r,c)] =
						A(r,c) * B.getData()[c];
						else 
						cD[C.offsetRowMajor(r,c)] = 0;
					}
				return C;
			} else if (B.isBiDiagonal()) {
				Matrix C(A.rows(), B.cols(), General);
				assert(C.isRowMajor() && A.cols() == B.rows());
				double * cD = C.getModData();
				const double * d = B.getData();
				const double * e = B.getOffDiagonal();;
				unsigned int colLimit = min(A.cols(), B.minDim());
				assert(colLimit != 0);
				if (B.isUpper() ) {
					for ( int r = 0; r < min(C.rows(), A.rows()); r++) {
						cD[C.offsetRowMajor(r,0)] = A(r,0) * d[0];
						for (unsigned int c = 1; c < colLimit; c++) 
							cD[C.offsetRowMajor(r,c)] = A(r,c) * d[c] + 
							A(r,c-1) * e[c-1];
						for (int c = colLimit; c < C.cols(); c++) 
							cD[C.offsetRowMajor(r,c)] = 0;
					}
					for (int r = C.rows(); r < A.rows(); r++) 
						for (int c = 0; c < C.cols(); c++)
							cD[C.offsetRowMajor(r,c)] = 0;
					return C;
				} else {
					for (int r = 0; r < min(C.rows(), A.rows()); r++) {
						for (unsigned int c = 0; c < colLimit-1; c++) 
							cD[C.offsetRowMajor(r,c)] = A(r,c) * d[c] + 
							A(r,c+1) * e[c];
						unsigned int c = colLimit - 1;
						cD[C.offsetRowMajor(r,c)] = A(r,c) * d[c];
						for (int c = colLimit; c < C.cols(); c++) 
							cD[C.offsetRowMajor(r,c)] = 0;
					}
					for (int r = C.rows(); r < A.rows(); r++) 
						for (int c = 0; c < C.cols(); c++)
							cD[C.offsetRowMajor(r,c)] = 0;
					return C;
				}
			} else if(B.isTriDiagonal()) {
				Matrix C(A.rows(), B.cols(), General);
				assert(C.isRowMajor() && A.cols() == B.rows());
				double * cD = C.getModData();
				const double * d = B.getData();
				const double * e = B.getOffDiagonal();;
				unsigned int colLimit = min(A.cols(), B.minDim());
				assert(colLimit != 0);
				for ( int r = 0; r < min(C.rows(), A.rows()); r++) {
					cD[C.offsetRowMajor(r,0)] = A(r,0) * d[0] + A(r,1) * e[0];
					for (unsigned int c = 1; c < colLimit - 1; c++) 
						cD[C.offsetRowMajor(r,c)] = A(r,c) * d[c] + 
						A(r,c-1) * e[c-1] + A(r,c + 1) * e[c];
					unsigned int c = colLimit - 1;
					cD[C.offsetRowMajor(r,c)] = A(r,c) * d[c] + A(r,c-1) * e[c-1];
					for (int c = colLimit; c < C.cols(); c++) 
						cD[C.offsetRowMajor(r,c)] = 0;
				}
				for (int r = C.rows(); r < A.rows(); r++) 
					for (int c = 0; c < C.cols(); c++)
						cD[C.offsetRowMajor(r,c)] = 0;
				return C;
			} else { MISSING;	}
		}
	} else {
		assert(isSparse());
		if (mat.isDense()) return (B.T() * A.T()).T();
		// bot matrices are sparse
		if (isDiagonal()) {
			if (mat.isDiagonal()) {
				Matrix C(A.rows(), B.cols(), Diagonal);
				uint limit = min(min(A.rows(), A.cols()),B.cols());
				double * cD = C.getModData();
				for (uint i = 0; i < limit; i++) 
					cD[i] = A.getData()[i] * B.getData()[i];	
				for (uint i = limit; i < C.getStorageSize(); i++)
					cD[i] = 0.0;
				return C;
			} else if (mat.isDense()) { return (B.T() * A.T()).T();
			} else { MISSING;	}
		} else {
			MISSING;
		}
	}
	return *this;
};
void Matrix::DiagMultCols(const double * diag){
	cout << "calling Diag Mult Cols" << endl;
	for (int r = 0; r < rows(); r++)
		for (int c = 0; c < cols(); c++) 
			data[offset(r,c)] *= diag[c];
}
void Matrix::DiagMultRows(const double * diag){ 
	cout << "calling Diag Mult Rows" << endl;
	for (int r = 0; r < rows(); r++)
		for (int c = 0; c < cols(); c++) 
			data[offset(r,c)] *= diag[r];
}
double  Matrix::OrthNorm() const { // max_{i!=j} |v_i . v_j| 
	if (isOrthogonal()) {
		Matrix A = isRowMajor() ? *this * T() : T() * *this;
		double res = 0.0;
		for (int r = 0; r < A.rows(); r++)
			for (int c = r + 1; c < A.cols(); c++) 
				res = fmax(A(r,c), res);
		return res;
	}
	assert(false);	return .0;
}
void Matrix::applyGivensRotationFromLeft(const unsigned int i, 
const unsigned int j, const double c, const double s) {
	// appliing RotationFrom right to RowMajor Matrix changes 2 rows
	unsigned int nSteps = cols();
	unsigned int step = isRowMajor() ? 1 : lda();;
	double * RS d = getModData();
	double * RS startI = d + offset(i,0); 
	double * RS startJ = d + offset(j,0); 
	rotate(nSteps, step, c, s, startI, startJ);
}
void Matrix::applyGivensRotationFromRight(const unsigned int i, 
const unsigned int j, const double c, const double s) {
	// appliing RotationFrom right to RowMajor Matrix changes 2 columns
	unsigned int nSteps = rows();
	unsigned int step = isRowMajor() ? lda() : 1;
	double * d = getModData();
	double * RS startI = d + offset(0,i); 
	double * RS startJ = d + offset(0,j); 
	rotate(nSteps, step, c, -s, startI, startJ);
}
void Matrix::rotate(const unsigned int nSteps, const unsigned int step, 
		const double c, const double s,	double * RS i, double * RS j) {
	for (unsigned int k = 0; k < nSteps; k++) {
		rot(*i, *j, c, s);
		i += step; j += step;
	}
}
void Matrix::killEntry(const unsigned int i, const unsigned int j, 
				 const unsigned int q, bool fromLeft) {
 	//bi.killEntry(1,0,1, false);
	double * d = getModData();
	double & b = d[offset(i,j)];
	double a = d[fromLeft ? offset(q,j) : offset(i,q)];
	double c,s;
	
	getCS(a, b, c, s);
//	cout << a << " " << b << " " << c << " "<< s << endl;
	assert(fabs(c*c + s*s - 1) < 5e-16);
	if (fromLeft) {
		assert(q != i);
		applyGivensRotationFromLeft (q, i, c, s);
	}	else {
		assert( j != q);
	   applyGivensRotationFromRight(j, q, c, s);
	}
	b = 0;
}
void Matrix::changeColSign(const unsigned int i) {
	int nSteps = rows();
	unsigned int step = isRowMajor() ? cols() : 1;
	double * d = getModData();
	double * RS startI = d + offset(0,i);
	for (int i = 0; i < nSteps; i++) {
		*startI = - *startI;
		startI += step;
	}
}


std::ostream& operator<<(std::ostream& os, const Matrix & m) {
	os << "\n" << m.rows() << "-by-" << m.cols() << " ld=" << m.lda() <<" Matrix ( ";
	if (m.order == RowMajor) os << "Row"; else os << "Col"; os << "Major, "; 
	switch (m.type) {
		case General    : { os << "General    , "; break;}
		case Orthogonal : { os << "Orthogonal , "; break;}
		case Diagonal   : { os << "Diagonal   , "; break;}
		case BiDiagonal : { os << "BiDiagonal , "; break;}
		case TriDiagonal: { os << "TriDiagonal, "; break;}
		case GivensRotations : {os << "GivensRotations, "; break;};
		case ElementaryReflectors : { os << "ElementaryReflectors, "; break; };
	}

	os << " Count=" << m.count() << " " << m.getData();
	streamsize p = cout.precision();
	cout.precision(16);
	if (m.isOrthogonal()) 
		os << " ortNorm=" << m.OrthNorm() << ")\n";
	else
		os << " nrm2=" <<  m.nrm2() << ")\n";
	cout.precision(p);

	if (m.data) {
		const int w = 15;
		if (m.isDense() && !m.isReflector()) {
			if (m.order == RowMajor) {
				for (int i = 0; i < m.rows(); i++) { 
					for (int j = 0; j < m.cols(); j++) 
						os << setw(w) << m.data[m.offsetRowMajor(i,j)] << " ";
					os << "\n";	
				}
			} else {
				for (int i = 0; i < m.rows(); i++) { 
					for (int j = 0; j < m.cols(); j++) 
						os << setw(w) << m.data[m.offsetColMajor(i,j)] << " ";
					os << "\n";	
				}
			}
		} else {
			if (m.isDiagonal() ) { 
				for (int i = 0; i < m.minDim(); i++) 
					os << m.data[i] << " ";
			} else if (m.isBiDiagonal()) {
				const double * e = m.getOffDiagonal();
				for (int i = 0; i < m.rows(); i++) { 
					for (int j = 0; j < m.cols(); j++) {
						double d = 0;
						if (m.isUpper()  && j  == i+1) d = e[i];
						if (!m.isUpper() && j+1== i  ) d = e[j];
						if (i == j) d = m.data[i];
						os << setw(w) << d << " ";
					}
					os << "\n";	
				}
			} else if (m.isTriDiagonal()) {
				const double * e = m.getOffDiagonal();
				for (int i = 0; i < m.rows(); i++) { 
					for (int j = 0; j < m.cols(); j++) {
						double d = 0;
						if (j   == i+1) d = e[i];
						if (j+1 == i  ) d = e[j];
						if (i   == j  ) d = m.data[i];
						os << setw(w) << d << " ";
					}
					os << "\n";	
				}
			} else if (m.isReflector()) {
				char symbol = m.isQ() ? 'Q' : 'P';
				os << "tau" << symbol << " = ";
				for (int i = 0; i < m.minDim(); i++) 
					os << m.data[i] << " ";
				os << endl;
				os << "reflector Storage = " << m.relflectorStorage << endl;
				os << *m.relflectorStorage << endl;
				
			} else {		MISSING 		}
		}
	} else {
		os << "No storage is allocated.";
	}
	os << endl;
	return os;
}
void Matrix::makeIdentity() {
	assert(isDense());
	double * d = isFullMatrix() ? getModData() : data;
	fill(0);
	for (int i = 0; i < minDim(); i++)
		d[offset(i,i)] = 1.0;
	makeOrthogonal();
}
void Matrix::swapCols(const unsigned int i, const unsigned int j){
	assert(isDense());
	double * RS d = getModData();
	double * RS startI = d + offset(0,i); 
	double * RS startJ = d + offset(0,j);
	unsigned int step = isColMajor() ? 1 : lda(); 
	for (int k = 0; k < rows(); k++){
		double tmp = *startI; 
		*startI = *startJ; 
		*startJ = tmp;
		startI += step;
		startJ += step;
	}
}
void Matrix::swapRows(const unsigned int i, const unsigned int j){
	assert(isDense());
	double * RS d = getModData();
	double * RS startI = d + offset(i,0); 
	double * RS startJ = d + offset(j,0);
	unsigned int step = isRowMajor() ? 1 : lda(); 
	for (int k = 0; k < cols(); k++){
		double tmp = *startI; 
		*startI = *startJ; 
		*startJ = tmp;
		startI += step;
		startJ += step;
	}

}
void Matrix::flipCols(){// columns flipped in the left-right direction
	if (isDense()) {
		for (int i = 0; i < cols() / 2; i++) {
			swapCols(i, cols() - i - 1);
		}
	} else if (isDiagonal()) {
		if (cols() <= rows()) {
			double * s = getModData();
			for (int i = 0; i < cols() / 2; i++) {
				double tmp = s[i];
				s[i] = s[n - i -1]; 
				s[n - i -1] = tmp; 
			}
		} else { MISSING };
	} else { MISSING };
};
void Matrix::flipRows(){// rows    flipped in the up-down direction
	if (isDense()) {
		for (int i = 0; i < rows() / 2; i++) {
			swapRows(i, rows() - i - 1);
		}
	} else if (isDiagonal()) {
		if (rows() <= cols()) {
			double * s = getModData();
			for (int i = 0; i < rows() / 2; i++) {
				double tmp = s[i];
				s[i] = s[n - i -1]; 
				s[n - i -1] = tmp; 
			}
		} else { MISSING };
	} else { MISSING };
};
void Matrix::solve(const double * RS b, const int ldb, double * RS x, 
		const int ldx,const double alpha ) const {
	if (isBiDiagonal()) {
		int n = cols();
		const double * RS d = getData();
		const double * RS e = getOffDiagonal();
		if (isUpper()) {
			if (ldb == 1 && ldx == 1) {
				x[n-1] = b[n-1] / d[n-1];
				if (alpha != 1.0 ) {
					for (int i = n-2 ; i >= 0; i--) {
						x[i] = (b[i] - x[i+1] * e[i])/d[i];
						x[i+1] *= alpha; 
					}
					x[0] *= alpha;
				} else {
					for (int i = n-2 ; i >= 0; i--) {
						x[i] = (b[i] - x[i+1] * e[i])/d[i];
					}
				}
			} else {
				x[(n-1) * ldx] = b[(n-1) * ldb] / d[n-1];
				if (alpha != 1.0 ) {
					for (int i = n-2 ; i >= 0; i--) {
						x[i * ldx] = (b[i * ldb] - x[(i+1) * ldx] * e[i])/d[i];
						x[(i+1) * ldx] *= alpha; 
					}
					x[0] *= alpha; 
				} else {
					for (int i = n-2 ; i >= 0; i--)
						x[i * ldx] = (b[i * ldb] - x[(i+1) * ldx] * e[i])/d[i];
				}
			} 
		} else {
			if (ldb == 1 && ldx == 1) {
				x[0] = b[0] / d[0];
				if (alpha != 1.0 ) {
					for (int i = 1 ; i < n ; i++) {
						x[i] = (b[i] - x[i-1] * e[i-1]) / d[i];
						x[i-1] *= alpha; 
					}
					x[n-1] *= alpha; 
				} else {
					for (int i = 1 ; i < n ; i++) 
						x[i] = (b[i] - x[i-1] * e[i-1]) / d[i];
				}
			} else {
				x[0] = b[0] / d[0];
				if (alpha != 1.0 ) {
					for (int i = 1 ; i < n ; i++) {
						x[i * ldx] = (b[i * ldb] - x[(i-1) * ldx] * e[i-1]) / d[i];
						x[(i-1) * ldx] *= alpha; 
					}
					x[(n-1) * ldx] *= alpha; 
				} else {
					for (int i = 1 ; i < n ; i++) 
						x[i * ldx] = (b[i * ldb] - x[(i-1) * ldx] * e[i-1]) / d[i];
				}

			} 
		}

	}	else { MISSING };
} // solves A x = b
void Matrix::setRotations(const int maxNumberOfRotations) {
	
}
void Matrix::deflate     (Matrix & L, Matrix & R){
	const int       dim = minDim();
	assert(isBiDiagonal() && isUpper() && dim >= 3);
	assert(isSquare() && L.isSquare() && R.isSquare() && rows() == L.rows());
	double * RS d = data;
	double * RS e = data + std::min(N,M);
	if (fabs(d[0]) < 1e-20) {
		double tmp = e[0]; e[0] = 0;
		double c,s;

	} else if (fabs(d[dim - 1] < 1e-20)) {

	}

} 
void Matrix::deflateLeft(Matrix & L) {
	assert(isBiDiagonal() && isUpper());
	const int       dim = minDim();
	double * RS d = data;
	double * RS e = data + std::min(N,M);
	assert(fabs(d[0]) < 1e-20);
	double tmp = e[0]; e[0] = 0;
	double c,s;
	for (int i = 1; i < dim - 1; i++) {
		getCS(d[i], tmp, c, s);
//		rot  (d[i], tmp, c, s);//	assert(fabs(tmp) < 1e-15);
		d[i] = d[i] * c + tmp * s;
		tmp = 0;
		rot  (e[i], tmp, c, s);
		L.applyGivensRotationFromLeft(i, 0, c, s);
	}
	int i = dim - 1;
	getCS(d[i], tmp, c, s);
	rot(  d[i], tmp, c, s);
	L.applyGivensRotationFromLeft(i, 0, c, s);
}
void Matrix::deflateRight(Matrix & R) {
	const int       dim = minDim();
	assert(isBiDiagonal() && isUpper() && dim >= 2);
	double * RS d = data;
	double * RS e = data + std::min(N,M);
	assert(fabs(d[dim-1]) < 1e-20);
	double tmp = e[dim-2]; e[dim-2] = 0;
	double c,s;
	for (int i = dim - 2; i > 0 ; i--) {
		getCS(d[i], tmp, c, s);
//		rot  (d[i], tmp, c, s);//	assert(fabs(tmp) < 1e-15);
		d[i] = d[i] * c + tmp * s;
		tmp = 0;
		rot  (e[i-1], tmp, c, s);
		R.applyGivensRotationFromRight(dim - 1, i, c, s);
	}
	int i = 0;
	getCS(d[i], tmp, c, s);
//	rot(  d[i], tmp, c, s);
	d[i] = d[i] * c + tmp * s;
	R.applyGivensRotationFromRight(dim - 1, i, c, s);
}


#ifndef NDEBUG
#include "UnitTester.h"
UNITTEST(TestMatrix);
#include <iostream>
using namespace std;
TestMatrix::TestMatrix() {
	setName 				("Test Matrix");
	setDescription  	("Test Matrix for correct bahaviour & memory allocation");
	setTestAuthor		("Yordan Madzhunkov");
	setDeveloperAuthor("Yordan Madzhunkov");
	setCompileTime(__DATE__, __TIME__); // each class that inherits this one should also have this line.
};
bool TestMatrix::test() const {

//////////////////////////////////
// test Diagonal times Diagonal //
//////////////////////////////////
	bool res = true;
	double a[] = {1,2,3,4,5,6};
	Matrix d(3, 3, Diagonal, a );

	d *= d;
	res &= d.getData()[0] == 1; 
	res &= d.getData()[1] == 4; 
	res &= d.getData()[2] == 9; assert(res);

	double a2[] = {3,2,2};
	Matrix d2 (3, 4, Diagonal, a2);
	d *= d2;

	res &= d.getData()[0] == 3; 
	res &= d.getData()[1] == 8; 
	res &= d.getData()[2] == 18; 
	res &= d.rows() == 3;
	res &= d.cols() == 4;
	assert(res);

	double a3[] = {0,1};
	Matrix d3 (4, 2, Diagonal, a3);
	
	d *= d3;
	res &= d.getData()[0] == 0; 
	res &= d.getData()[1] == 8; 
// uncommenting the next line should cause invalid read in valgrind
//	res &= d.getData()[2] == 0; 
	res &= d.rows() == 3;
	res &= d.cols() == 2;
	res &= d.getStorageSize() == 2 ;
	assert(res);

////////////////////////////////////
// Test Diagonal times Dense      //
////////////////////////////////////	
	double ones[] = { 1,1,1, 1,1,1, 1,1,1, 1,1,1, 
							1,1,1, 1,1,1, 1,1,1, 1,1,1, 
							1,1,1, 1,1,1, 1,1,1, 1,1,1};
	
	Matrix D6_6(6, 6, Diagonal, a);
	Matrix D3_3 = D6_6; D3_3.setView(0, 0, 3, 3);
	Matrix D3_6 = D6_6; D3_6.setView(0, 0, 3, 6);
	Matrix A6_6(6, 6, General , ones);
	Matrix A6_3 = A6_6; A6_3.setView(0, 0, 6, 3);

	Matrix TMP = A6_6 * D6_6;
	for (int i = 0; i < 6; i++) 
		for (int j = 0; j < 6; j++) 
			res &= TMP(i,j) == j+1;

	TMP = D6_6 * A6_6;
	for (int i = 0; i < 6; i++) 
		for (int j = 0; j < 6; j++) 
			res &= TMP(i,j) == i+1;

	TMP = A6_6 * D3_6.T();
	for (int i = 0; i < 6; i++) 
		for (int j = 0; j < 3; j++) 
			res &= TMP(i,j) == j+1;

	TMP = D3_6 * A6_6;
	for (int i = 0; i < 3; i++) 
		for (int j = 0; j < 6; j++) 
			res &= TMP(i,j) == i+1;

	TMP = D3_6 * A6_3;
	for (int i = 0; i < 3; i++) 
		for (int j = 0; j < 3; j++) 
			res &= TMP(i,j) == i+1;

	TMP = A6_3 * D3_6;
	for (int i = 0; i < 3; i++) { 
		for (int j = 0; j < 3; j++) 
			res &= TMP(i,j) == j+1;
		for (int j = 3; j < 6; j++) 
			res &= TMP(i,j) == 0  ;
	}

////////////////////////////////////
// Test Orthogonal times Diagonal //
////////////////////////////////////

//////////////////////////////////////
// Test Orthogonal times Orthogonal //
//////////////////////////////////////
	double or1[] = { 1.0/2, -sqrt(3)/2, sqrt(3)/2, 1.0/2};
	Matrix O1(2,2,Orthogonal, or1);
//	cout << O1 * O1.T() << endl;

////////////////////////////////
// Test General times General //
////////////////////////////////

	double A1[] = { 1, 2, 3, 4 };
	Matrix A(2,2, General, A1);
	double B1[] = { 2, 1, 0, 2 };
	Matrix B(2,2, General, B1);
	Matrix AAT  = A     * A.T();
	Matrix AB   = A     * B;
	Matrix ABT  = A     * B.T();
	Matrix ATB  = A.T() * B;
	Matrix ATBT = A.T() * B.T();
// | 1 2 |   | 2 1 |   | 2  5|
// | 3 4 | * | 0 2 | = | 6 11|
	double AB1[] = {2, 5, 6, 11};
	Matrix AB_ref(2,2, General, AB1); 

	double ab2[] = { 4, 4, 10, 8 };
	Matrix ABT_ref (2,2,General, ab2);

	double ab3[] = { 2, 7, 4, 10 };
	Matrix ATB_ref (2,2,General, ab3);

	double ab4[] = { 5, 6, 8, 8 };
	Matrix ATBT_ref (2, 2, General, ab4);
	
	double aa1[] = { 5, 11, 11, 25 };
	Matrix AAT_ref (2, 2, General, aa1);
	
	assert(res);
	AB   -=   AB_ref;	res &=   AB.nrm2() == 0; assert(res);
	ABT  -=  ABT_ref;	res &=  ABT.nrm2() == 0; assert(res);
	ATB  -=  ATB_ref;	res &=  ATB.nrm2() == 0; assert(res);
	ATBT -= ATBT_ref;	res &= ATBT.nrm2() == 0; assert(res);
	AAT  -=  AAT_ref;	res &=  AAT.nrm2() == 0; assert(res);
	res &= (A * B - AB_ref).nrm2() == 0; assert(res);
// Let's try with some rectangular matrices

// | 1 2 |   | 1 2 3 |   |  9 12 15 |
// | 3 4 | * | 4 5 6 | = | 19 26 33 |

	Matrix C (2, 3, General, a);
	double c2[] = { 1, 0, 0, 1, 0, 0};
	Matrix C2(3, 2, General, c2);
	double ctc[] = {17, 22, 27, 22, 29, 36, 27, 36, 45};
	Matrix CTC_ref(3, 3, General, ctc);
	res &= (C.T() * C - CTC_ref).nrm2() == 0; assert(res);
	assert(res);

	double ac1[] = {9, 12, 15, 19, 26, 33};
	Matrix AC_ref (2, 3, General, ac1);
	Matrix AC = A * C;
	res &= AC_ref(0,0) ==  9; assert(res);
	res &= AC_ref(0,1) == 12; assert(res);
	res &= AC_ref(0,2) == 15; assert(res);
	res &= AC_ref(1,0) == 19; assert(res);
	res &= AC_ref(1,1) == 26; assert(res);
	res &= AC_ref(1,2) == 33; assert(res);

	res &= AC(0,0) ==  9; assert(res);
	res &= AC(0,1) == 12; assert(res);
	res &= AC(0,2) == 15; assert(res);
	res &= AC(1,0) == 19; assert(res);
	res &= AC(1,1) == 26; assert(res);
	res &= AC(1,2) == 33; assert(res);

	assert(res);
	res &= (A * C - AC_ref).nrm2() == 0; assert(res);
	assert(res);
	res &= (C.T() * A.T() - AC_ref.T()).nrm2() < 3e-16;
	assert(res);
// TEST GIVEN'S ROTATIONS
 	double ar1[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
	17, 18, 19, 20, 21, 22, 23, 24, 25 };
	 Matrix ar(4, 4, General, ar1);
	Matrix ar2 = ar;
	ar.applyGivensRotationFromRight(0, 2, cos(1.2), sin(1.2));
	ar.applyGivensRotationFromLeft (0, 2, cos(1.2), sin(1.2));
	double ar1_ref[] = {
  -7.397972751594019e+00, 1.004510636862561e+01, 1.226496022993064e+01,   
	1.263390004951341e+01,-4.712484829387216e+00, 6.000000000000000e+00,
	7.196699711172847e+00, 8.000000000000000e+00,-2.649602299306423e-01,
	1.759499372832284e+00, 2.602027248405981e+00, 6.201367098511783e-01,
  -9.269935481311638e+00, 1.400000000000000e+01, 1.755187443472404e+01,
   1.600000000000000e+01 };
	Matrix ar_ref(4, 4, General, ar1_ref);
	res &= (ar - ar_ref).nrm2() < 1e-14;
	assert(res);
	//test rotations for ColMajor order
	ar2.changeOrder(); 
	ar2.applyGivensRotationFromRight(0, 2, cos(1.2), sin(1.2));
	ar2.applyGivensRotationFromLeft (0, 2, cos(1.2), sin(1.2));
	res &= (ar2 - ar_ref).nrm2() < 1e-14;
	assert(res);
// TEST BIDIAGONAL MATRICES	 
	Matrix Ab (4,3, General, ar1); Ab.makeColMajor();
	Matrix Bi (3,3, BiDiagonal, ar1);
	double abbi_ref[] = { 1, 14, 52, 2, 20, 60, 3, 26, 68, 4, 32, 76 };
	Matrix AbBi_ref(4,3, General, abbi_ref);
	res &= (Ab * Bi - AbBi_ref).nrm2() < 1e-15;
	Matrix Bi2 (3,5, BiDiagonal, ar1);
	res &= (Ab * Bi2)(2,3) == 0;
	double biabT_ref[] = { 21, 26, 31, 36, 55, 62, 69, 76, 27, 30, 33, 36};
	Matrix BiAbT (3,4, General, biabT_ref);
	res &= (Bi * Ab.T() - BiAbT).nrm2() < 1e-15;

	Matrix B3(4, 4, BiDiagonal, ar1);
	double x[8]; double x_ref[] = { -5.25, 1.25, -0.25, 0.25 };

	B3.solve(ones, 1, x, 1);
	for (int i = 0; i < 4; i++) { res &= x[i] == x_ref[i]; x[i] = 0; } 

	B3.solve(ones, 1, x, 1, 2);
	for (int i = 0; i < 4; i++) { res &= x[i] == x_ref[i] * 2; x[i] = 0; } 

	B3.solve(ones, 2, x, 1, 2);
	for (int i = 0; i < 4; i++) { res &= x[i] == x_ref[i] * 2; x[i] = 0; } 

	B3.solve(ones, 2, x, 2, 2);
	for (int i = 0; i < 4; i++) { res &= x[2*i] == x_ref[i] * 2; x[2*i] = 0; } 
	B3.transpose();
	double xt_ref[] = {1.0,  -2.0, 13.0/3, -22.0/3};

	B3.solve(ones, 1, x, 1, 1);
	for (int i = 0; i < 4; i++) { res &= x[i] == xt_ref[i]; x[i] = 0; } 
	assert(res);


// TEST TRIDIAGONAL MATRICES
	double t1[] = {1,2,3,4,5,10,11,12,13};
	Matrix T1(5, 5, TriDiagonal, t1);
	Matrix B2(5, 4, General,     ar1); B2.makeColMajor();
	double t1b2_ref[] = { 21,  76, 131, 186, 47, 162, 277, 392, 79, 209, 339,
							   469, 117, 262, 407, 552, 77, 167, 257, 347};
	Matrix T1B2(5,4,General, t1b2_ref);
	res &= (T1 * B2 - T1B2).nrm2() < 1e-15;
	assert(res);

// TEST MATRIX COL/ROW FLIPPING	
//	double f1[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
	Matrix F1(4, 3, General, ar1);	
	double f2[] = { 3, 2, 1, 6, 5, 4, 9, 8, 7, 12, 11, 10};
	Matrix F2(4, 3, General, f2);
	double f3[] = { 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1};
	Matrix F3(4, 3, General, f3);
	F1.flipCols();
	res &= (F1 - F2).nrm2() == 0;
	assert(res);
	F1.flipRows();
	res &= (F1 - F3).nrm2() == 0;
	assert(res);

///////////////////////////////////////////
//////////// SUBMATRIX TESTING ////////////
///////////////////////////////////////////
	assert(res);
	Matrix F1sub = F1;
	F1sub.setView(1,1,2,2);
	res &= fabs(sqrt(154) - F1sub.nrm2()) < 1e-15;
	F1sub.setView(1,1,2,1);
	res &= fabs(sqrt(8 * 8 + 5 * 5) - F1sub.nrm2())     < 1e-15; assert(res);
	res &= fabs(sqrt(8 * 8 + 5 * 5) - F1sub.T().nrm2()) < 1e-15; assert(res);

	// Mutliply two sub matrices
	F1sub.setView(1,2,2,1); 
	Matrix F1TF1 = F1sub.T() * F1sub;
	res &= F1TF1(0,0) == 65 && F1TF1.rows() == 1 && F1TF1.cols() == 1;

	
	F1sub.fill(0);
	F1sub.resetView();
	Matrix TL = T1B2; TL.setView(0,0,3,2);
	Matrix TR = T1B2; TR.setView(0,2,2,2);
	Matrix BL = T1B2; BL.setView(3,0,2,2);
	Matrix BR = T1B2; BR.setView(2,2,3,2);
	BR.copyOnlyData( TL * TR ); // copy from col to row major 
	BL.copyOnlyData( TR      ); // copy from row to row major
	double t1b2_ref2[] = {
       21,   76,    131,     186, 
       47,  162,    277,     392, 
		 79,  209,  23803,   33698,         
		131,  186,	51031,   72246, 
		277,  392,	68242,   96622 	};
	Matrix T1B2_ref(5,4,General, t1b2_ref2);
	res &= (T1B2 - T1B2_ref).nrm2() == 0 ;

	Matrix B3T = B3; B3T.setView(0,0,2,2);
	Matrix B3B = B3; B3B.setView(2,2,2,2);
	res &= B3B.getOffDiagonal()[0] == 7; assert(res);
	Matrix B3Copy (2,2,BiDiagonal);
	Matrix B3Copy2(2,2,BiDiagonal);
	B3Copy.copyOnlyData(B3T);
	res &= B3Copy.getOffDiagonal()[0] == 5; assert(res);
	B3Copy2.copyOnlyData(B3B);
	res &= B3Copy2.getOffDiagonal()[0] == 7; assert(res);
	res &= (D3_3 - D3_3).nrm2() == 0;

// DEFLATING A BIDIAGONAL MATRIX
	double t2[] = {1, 2, 3, 4, 0, 6, 7, 8, 9, 10, 1.0/8, 2.0/8, 3.0/8,
						4.0/8, 5.0/8, 6.0/8, 7.0/8, 8.0/8, 9.0/8};
	Matrix T2(10, 10, BiDiagonal, t2);
//	cout << T2 << endl;
	Matrix KT = T2; KT.setView(0,0,5,5);

	int k = 6;
	Matrix KB = T2; KB.setView(4,4,k,k); 
	Matrix KB2 = Matrix(k, k, BiDiagonal);
	KB2.copyOnlyData(KB);

	Matrix L(k,k,General   ); L.makeIdentity();
	KB.deflateLeft(L);
	
	res &= KB.getData()[0] == 0 && KB.getOffDiagonal()[0] == 0;
	res &= (L.T() * KB -     KB2).nrm2() < 1e-14;
	res &= (        KB - L * KB2).nrm2() < 1e-14;
//	cout << KB << endl;

	k = 5;
	Matrix KT2 = Matrix(k,k,BiDiagonal);
	KT2.copyOnlyData(KT);
	Matrix R(k,k,General);
	R.makeIdentity();
	KT.deflateRight(R);
	res &= (KT * R.T() - KT2    ).nrm2() < 1e-14;
	res &= (KT         - KT2 * R).nrm2() < 1e-14;


	double t3[] = {0, 2, 3, 4, 5, 6, 7, 8, 9, 0, 1.0/80, 2.0/80, 3.0/80,
						4.0/80, 5.0/80, 6.0/80, 7.0/80, 8.0/80, 9.0/80};
	Matrix T3    (10, 10, BiDiagonal, t3);
	Matrix T3_org(10, 10, BiDiagonal, t3);
	Matrix R1    (10, 10, General   ); R1.makeIdentity();

	T3.deflateRight(R1);
//	cout << T3 << endl;
	res &= (T3_org * R1 - T3         ).nrm2() < 1e-14;
	res &= (T3_org      - T3 * R1.T()).nrm2() < 1e-14;
	

	Matrix R1Sub = R1; R1Sub.setView(4,0,6,4);
	R1Sub.makeIdentity();	
//	cout << R1 << endl;

// 
//	Matrix CT = (C.T());
//	C.changeOrder();
//	cout << C * C2;
//	cout << C * C.T();
//	cout << C.T() * C;
//	cout << C * CT;
	

	return res;
}
TestMatrix testMatrix;
#endif
